#include "pid_manager.h"
#include <stdlib.h>
#include <string.h>

// 全局初始化参数数组（对应三阶PID的三个阶段）
PID_InitParams g_pid_init_params[PID_ORDER_THIRD] = {
    // 第一阶段参数
    {
        .kp = 1.0f,                    // 比例系数
        .ki = 0.0f,                    // 积分系数
        .kd = 0.0f,                    // 微分系数
        .max_output = 1000.0f,         // 最大输出
        .min_output = -1000.0f,        // 最小输出
        .max_integral = 1000.0f,       // 最大积分
        .min_integral = -1000.0f,      // 最小积分
        .max_derivative = 1000.0f,     // 最大微分
        .min_derivative = -1000.0f,    // 最小微分
        .separation_threshold = 1000.0f // 积分分离阈值
    },
    // 第二阶段参数
    {
        .kp = 1.0f,
        .ki = 0.0f,
        .kd = 0.0f,
        .max_output = 1000.0f,
        .min_output = -1000.0f,
        .max_integral = 1000.0f,
        .min_integral = -1000.0f,
        .max_derivative = 1000.0f,
        .min_derivative = -1000.0f,
        .separation_threshold = 1000.0f
    },
    // 第三阶段参数
    {
        .kp = 1.0f,
        .ki = 0.0f,
        .kd = 0.0f,
        .max_output = 1000.0f,
        .min_output = -1000.0f,
        .max_integral = 1000.0f,
        .min_integral = -1000.0f,
        .max_derivative = 1000.0f,
        .min_derivative = -1000.0f,
        .separation_threshold = 1000.0f
    }
};

// 初始化PID管理器
void pid_manager_init(PID_Manager* manager, PID_Order order) {
    manager->order = order;
    
    switch (order) {
        case PID_ORDER_FIRST:
            // 初始化一阶PID控制器
            pid_init(&manager->controller.first_order.pid,
                    g_pid_init_params[0].kp, g_pid_init_params[0].ki, g_pid_init_params[0].kd,
                    g_pid_init_params[0].max_output, g_pid_init_params[0].min_output,
                    g_pid_init_params[0].max_integral, g_pid_init_params[0].min_integral,
                    g_pid_init_params[0].max_derivative, g_pid_init_params[0].min_derivative,
                    g_pid_init_params[0].separation_threshold);
            manager->controller.first_order.output = 0.0f;
            break;
            
        case PID_ORDER_SECOND:
            // 初始化二阶PID控制器的第一级
            pid_init(&manager->controller.second_order.pid1,
                    g_pid_init_params[0].kp, g_pid_init_params[0].ki, g_pid_init_params[0].kd,
                    g_pid_init_params[0].max_output, g_pid_init_params[0].min_output,
                    g_pid_init_params[0].max_integral, g_pid_init_params[0].min_integral,
                    g_pid_init_params[0].max_derivative, g_pid_init_params[0].min_derivative,
                    g_pid_init_params[0].separation_threshold);
            // 初始化二阶PID控制器的第二级
            pid_init(&manager->controller.second_order.pid2,
                    g_pid_init_params[1].kp, g_pid_init_params[1].ki, g_pid_init_params[1].kd,
                    g_pid_init_params[1].max_output, g_pid_init_params[1].min_output,
                    g_pid_init_params[1].max_integral, g_pid_init_params[1].min_integral,
                    g_pid_init_params[1].max_derivative, g_pid_init_params[1].min_derivative,
                    g_pid_init_params[1].separation_threshold);
            manager->controller.second_order.output1 = 0.0f;
            manager->controller.second_order.output2 = 0.0f;
            manager->controller.second_order.final_output = 0.0f;
            break;
            
        case PID_ORDER_THIRD:
            // 初始化三阶PID控制器的第一级
            pid_init(&manager->controller.third_order.pid1,
                    g_pid_init_params[0].kp, g_pid_init_params[0].ki, g_pid_init_params[0].kd,
                    g_pid_init_params[0].max_output, g_pid_init_params[0].min_output,
                    g_pid_init_params[0].max_integral, g极id_init_params[0].min_integral,
                    g_pid_init_params[0].max_derivative, g_pid_init_params[0].min_derivative,
                    g_pid_init_params[0].separation_threshold);
            // 初始化三阶PID控制器的第二级
            pid_init(&manager->controller.third_order.pid2,
                    g_pid_init_params[1].kp, g_pid_init_params[1].ki, g_pid_init_params[1].kd,
                    g_pid_init_params[1].max_output, g_pid_init_params[1].min_output,
                    g_pid_init_params[1].max_integral, g_pid_init_params[1].min_integral,
                    g_pid_init_params[1].max_derivative, g_pid_init_params[1].min_derivative,
                    g_pid_init_params[1].separation_threshold);
            // 初始化三阶PID控制器的第三级
            pid_init(&manager->controller.third_order.pid3,
                    g_pid_init_params[2].极p, g_pid_init_params[2].ki, g_pid_init_params[2].kd,
                    g_pid_init_params[2].max_output, g_pid_init_params[2].min_output,
                    g_pid_init_params[2].max_integral, g_pid_init_params[2].min_integral,
                    g_pid_init_params[2].max_derivative, g_pid_init_params[2].min_derivative,
                    g_pid_init_params[2].separation_threshold);
            manager->controller.third_order.output1 = 0.0f;
            manager->controller.third_order.output2 = 0.0f;
            manager->controller.third_order.output3 = 0.0f;
            manager->controller.third_order.final_output = 0.0f;
            break;
    }
}

// 设置全局PID参数
void pid_manager_set_global_params(uint8_t index, float kp, float ki, float kd,
                                 float max_output, float min_output,
                                 float max_integral, float min_integral,
                                 float max_derivative, float min_derivative,
                                 float separation_threshold) {
    if (index >= 3) return; // 索引不能超过2（对应三阶PID的三个阶段）
    
    g_pid_init_params[index].kp = kp;
    g_pid_init_params[index].ki = ki;
    g_pid_init_params[index].kd = kd;
    g_pid_init_params[index].max_output = max_output;
    g_pid_init_params[index].min_output = min_output;
    g_pid_init_params[index].max_integral = max_integral;
    g_pid_init_params[index].min_integral = min_integral;
    g_pid_init_params[index].max_derivative = max_derivative;
    g_pid_init_params[index].min_derivative = min_derivative;
    g_pid_init_params[index].separation_threshold = separation_threshold;
}

// 更新PID参数
void pid_manager_update_params(PID_Manager* manager, uint8_t stage, 
                             float kp, float ki, float kd,
                             float max_output, float min_output,
                             float max_integral, float min_integral,
                             float max_derivative, float min_derivative,
                             float separation_threshold) {
    switch (manager->order) {
        case PID_ORDER_FIRST:
            if (stage == 0) {
                // 更新一阶PID参数
                pid_init(&manager->controller.first_order.pid,
                        kp, ki, kd,
                        max_output, min_output,
                        max_integral, min_integral,
                        max_derivative, min_derivative,
                        separation_threshold);
            }
            break;
            
        case PID_ORDER_SECOND:
            if (stage == 0) {
                // 更新二阶PID第一级参数
                pid_init(&manager->controller.second_order.pid1,
                        kp, ki, kd,
                        max_output, min_output,
                        max_integral, min_integral,
                        max_derivative, min_derivative,
                        separation_threshold);
            } else if (stage == 1) {
                // 更新二阶PID第二级参数
                pid_init(&manager->controller.second_order.pid2,
                        kp, ki, kd,
                        max_output, min_output,
                        max_integral, min_integral,
                        max_derivative, min_derivative,
                        separation_threshold);
            }
            break;
            
        case PID_ORDER_THIRD:
            if (stage == 0) {
                // 更新三阶PID第一级参数
                pid_init(&manager->controller.third_order.pid1,
                        kp, ki, kd,
                        max_output, min_output,
                        max_integral, min_integral,
                        max_derivative, min_derivative,
                        separation_threshold);
            } else if (stage == 1) {
                // 更新三阶PID第二级参数
                pid_init(&manager->controller.third_order.pid2,
                        kp, ki, kd,
                        max_output, min_output,
                        max_integral, min_integral,
                        max_derivative, min_derivative,
                        separation_threshold);
            } else if (stage == 2) {
                // 更新三阶PID第三级参数
                pid_init(&manager->controller.third_order.pid3,
                        kp, ki, kd,
                        max_output, min_output,
                        max_integral, min_integral,
                        max_derivative, min_derivative,
                        separation_threshold);
            }
            break;
    }
}

// 更新PID控制器
float pid_manager_update(PID_Manager* manager, float setpoint, float current_value, float current_time) {
    switch (manager->order) {
        case PID_ORDER_FIRST:
            // 更新一阶PID控制器
            pid_update(&manager->controller.first_order.pid, setpoint, current_value, current_time);
            manager->controller.first_order.output = manager->controller.first_order.pid.last_output;
            return manager->controller.first_order.output;
            
        case PID_ORDER_SECOND:
            // 更新二阶PID第一级控制器
            pid_update(&manager->controller.second_order.pid1, setpoint, current_value, current_time);
            manager->controller.second_order.output1 = manager->controller.second_order.pid1.last_output;
            
            // 更新二阶PID第二级控制器
            pid_update(&manager->controller.second_order.pid2, manager->controller.second_order.output1, current_value, current_time);
            manager->controller.second_order.output2 = manager->controller.second_order.pid2.last_output;
            manager->controller.second_order.final_output = manager->controller.second_order.output2;
            
            return manager->controller.second_order.final_output;
            
        case PID_ORDER_THIRD:
            // 更新三阶PID第一级控制器
            pid_update(&manager->controller.third_order.pid1, setpoint, current_value, current_time);
            manager->controller.third_order.output1 = manager->controller.third_order.pid1.last_output;
            
            // 更新三阶PID第二级控制器
            pid_update(&manager->controller.third_order.pid2, manager->controller.third_order.output1, current_value, current_time);
            manager->controller.third_order.output2 = manager->controller.third_order.pid2.last_output;
            
            // 更新三阶PID第三级控制器
            pid_update(&manager->controller.third_order.pid3, manager->controller.third_order.output2, current_value, current_time);
            manager->controller.third_order.output3 = manager->controller.third_order.pid3.last_output;
            manager->controller.third_order.final_output = manager->controller.third_order.output3;
            
            return manager->controller.third_order.final_output;
            
        default:
            return 0.0f;
    }
}

// 获取PID输出
float pid_manager_get_output(PID_Manager* manager, uint8_t stage) {
    switch (manager->order) {
        case PID_ORDER_FIRST:
            if (stage == 0) return manager->controller.first_order.output;
            break;
            
        case PID_ORDER_SECOND:
            if (stage == 0) return manager->controller.second_order.output1;
            else if (stage == 1) return manager->controller.second_order.output2;
            break;
            
        case PID_ORDER_THIRD:
            if (stage == 0) return manager->controller.third_order.output1;
            else if (stage == 1) return manager->controller.third_order.output2;
            else if (stage == 2) return manager->controller.third_order.output3;
            break;
    }
    
    return 0.0f;
}

// 重置PID控制器
void pid_manager_reset(PID_Manager* manager) {
    switch (manager->order) {
        case PID_ORDER_FIRST:
            // 重置一阶PID控制器
            pid_init(&manager->controller.first_order.pid,
                    manager->controller.first_order.pid.kp,
                    manager->controller.first_order.pid.ki,
                    manager->controller.first_order.pid.kd,
                    manager->controller.first_order.pid.max_output,
                    manager->controller.first_order.pid.min_output,
                    manager->controller.first_order.pid.max_integral,
                    manager->controller.first_order.pid.min_integral,
                    manager->controller.first_order.pid.max_derivative,
                    manager->controller.first_order.pid.min_derivative,
                    manager->controller.first_order.pid.separation_threshold);
            manager->controller.first_order.output = 0.0f;
            break;
            
        case PID_ORDER_SECOND:
            // 重置二阶PID第一级控制器
            pid_init(&manager->controller.second_order.pid1,
                    manager->controller.second_order.pid1.kp,
                    manager->controller.second_order.pid1.ki,
                    manager->controller.second_order.pid1.kd,
                    manager->controller.second_order.pid1.max_output,
                    manager->controller.second_order.pid1.min_output,
                    manager->controller.second_order.pid1.max_integral,
                    manager->controller.second_order.pid1.min_integral,
                    manager->controller.second_order.pid1.max_derivative,
                    manager->controller.second_order.pid1.min_derivative,
                    manager->controller.second_order.pid1.separation_threshold);
            // 重置二阶PID第二级控制器
            pid_init(&manager->controller.second_order.pid2,
                    manager->controller.second_order.pid2.kp,
                    manager->controller.second_order.pid2.ki,
                    manager->controller.second_order.pid2.kd,
                    manager->controller.second_order.pid2.max_output,
                    manager->controller.second_order.pid2.min_output,
                    manager->controller.second_order.pid2.max_integral,
                    manager->controller.second_order.pid2.min_integral,
                    manager->controller.second_order.pid2.max_derivative,
                    manager->controller.second_order.pid2.min_derivative,
                    manager->controller.second_order.pid2.separation_threshold);
            manager->controller.second_order.output1 = 0.0f;
            manager->controller.second_order.output2 = 0.0f;
            manager->controller.second_order.final_output = 0.0f;
            break;
            
        case PID_ORDER_THIRD:
            // 重置三阶PID第一级控制器
            pid_init(&manager->controller.third_order.pid1,
                    manager->controller.third_order.pid1.kp,
                    manager->controller.third_order.pid1.ki,
                    manager->controller.third_order.pid1.kd,
                    manager->controller.third_order.pid1.max_output,
                    manager->controller.third_order.pid1.min_output,
                    manager->controller.third_order.pid1.max_integral,
                    manager->controller.third_order.pid1.min_integral,
                    manager->controller.third_order.pid1.max_derivative,
                    manager->controller.third_order.pid1.min_derivative,
                    manager->controller.third_order.pid1.separation_threshold);
            // 重置三阶PID第二级控制器
            pid_init(&manager->controller.third_order.pid2,
                    manager->controller.third_order.pid2.kp,
                    manager->controller.third_order.pid2.ki,
                    manager->controller.third_order.pid2.kd,
                    manager->controller.third_order.pid2.max_output,
                    manager->controller.third_order.pid2.min_output,
                    manager->controller.third_order.pid2.max_integral,
                    manager->controller.third_order.pid2.min_integral,
                    manager->controller.third_order极pid2.max_derivative,
                    manager->controller.third_order.pid2.min_derivative,
                    manager->controller.third_order.pid2.separation_threshold);
            // 重置三阶PID第三级控制器
            pid_init(&manager->controller.third_order.pid3,
                    manager->controller.third_order.pid3.kp,
                    manager->controller.third_order.pid3.ki,
                    manager->controller.third_order.pid3.kd,
                    manager->controller.third_order.pid3.max_output,
                    manager->controller.third_order.pid3.min_output,
                    manager->controller.third_order.pid3.max_integral,
                    manager->controller.third_order.pid3.min_integral,
                    manager->controller.third_order.pid3.max_derivative,
                    manager->controller.third_order.pid3.min_derivative,
                    manager->controller.third_order.pid3.separation_threshold);
            manager->controller.third_order.output1 = 0.0f;
            manager->controller.third_order.output2 = 0.0f;
            manager->controller.third_order.output3 = 0.0f;
            manager->controller.third_order.final_output = 0.0f;
            break;
    }
}
